#include <iostream>
#include <vector>
#include <opencv2/core.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include <librealsense2/rs.hpp>

const static int DEPTH_WIDTH = 1280;
const static int DEPTH_HEIGHT = 720;
const static int VIDEO_WIDTH = 1920;
const static int VIDEO_HEIGHT = 1080;
const static int FPS = 30;
const static float SCALE = 1920.0 / 1280;

// 非对齐版本
void fun1()
{
    rs2::pipeline p;
    rs2::config cof;
    // 设置帧率分辨率及颜色格式
    cof.enable_stream(RS2_STREAM_DEPTH, DEPTH_WIDTH, DEPTH_HEIGHT, RS2_FORMAT_Z16, FPS);
    cof.enable_stream(RS2_STREAM_COLOR, VIDEO_WIDTH, VIDEO_HEIGHT, RS2_FORMAT_BGR8, FPS);
    p.start(cof);

    cv::namedWindow("color", cv::WINDOW_AUTOSIZE);
    cv::namedWindow("depth", cv::WINDOW_AUTOSIZE);

    rs2::colorizer clzr;
    while (1)
    {
        // 分别获取视频和深度信息
        rs2::frameset frames = p.wait_for_frames();
        rs2::depth_frame depth = frames.get_depth_frame();
        rs2::video_frame video = frames.get_color_frame();
        // 深度信息转彩色图
        rs2::video_frame colorDepth = clzr.colorize(depth);

        // 转Mat对象
        cv::Mat depthImg(
            cv::Size(colorDepth.get_width(),
                     colorDepth.get_height()),
            CV_8UC3,
            (void *)colorDepth.get_data(),
            cv::Mat::AUTO_STEP);
        cv::Mat colorImg(
            cv::Size(video.get_width(),
                     video.get_height()),
            CV_8UC3,
            (void *)video.get_data(),
            cv::Mat::AUTO_STEP);

        cv::imshow("color", colorImg);
        cv::imshow("depth", depthImg);

        if (cv::waitKey(1) == 27)
            break;
        std::cout << "中心距离：" << depth.get_distance(VIDEO_WIDTH / 2, VIDEO_HEIGHT / 2) << std::endl;
    }
    p.stop();
}

// 对齐版本
void fun2()
{
    rs2::pipeline p;
    rs2::config cof;
    cof.enable_stream(RS2_STREAM_DEPTH, DEPTH_WIDTH, DEPTH_HEIGHT, RS2_FORMAT_Z16, FPS);
    cof.enable_stream(RS2_STREAM_COLOR, VIDEO_WIDTH, VIDEO_HEIGHT, RS2_FORMAT_BGR8, FPS);
    p.start(cof);

    cv::namedWindow("img", cv::WINDOW_AUTOSIZE);
    // 导入级联分类器
    cv::CascadeClassifier face("/usr/share/opencv4/haarcascades/haarcascade_frontalface_default.xml");
    std::vector<cv::Rect> faces;

    rs2::colorizer clzr;
    rs2::align alignToDepth(RS2_STREAM_DEPTH);
    rs2::align alignToColor(RS2_STREAM_COLOR);
    while (1)
    {
        rs2::frameset frames = p.wait_for_frames();
        rs2::frameset alignFrames = alignToColor.process(frames);
        rs2::depth_frame depth = alignFrames.get_depth_frame();
        rs2::video_frame video = alignFrames.get_color_frame();
        rs2::video_frame colorDepth = clzr.colorize(depth);

        cv::Mat depthImg(
            cv::Size(colorDepth.get_width(),
                     colorDepth.get_height()),
            CV_8UC3,
            (void *)colorDepth.get_data(),
            cv::Mat::AUTO_STEP);
        cv::Mat colorImg(
            cv::Size(video.get_width(),
                     video.get_height()),
            CV_8UC3,
            (void *)video.get_data(),
            cv::Mat::AUTO_STEP);
        // 推理
        face.detectMultiScale(colorImg, faces);

        cv::Mat ret;
        // 合并视频及深度色彩图
        cv::addWeighted(colorImg, 0.5, depthImg, 0.5, 0, ret);
        // 绘制推理结果
        for (const cv::Rect &var : faces)
            cv::rectangle(ret, var, cv::Scalar(0, 0, 255), 4);
        cv::resize(ret, ret, cv::Size(1280, 720), cv::INTER_AREA);
        cv::imshow("img", ret);
        if (cv::waitKey(1) == 27)
            break;
        std::cout << "中心距离：" << depth.get_distance(VIDEO_WIDTH / 2, VIDEO_HEIGHT / 2) << std::endl;
    }
    p.stop();
}

int main(int argc, char const *argv[])
{
    fun2();
    return 0;
}
